#include "v4l2_capture.h"
#include <opencv2/opencv.hpp>
#include <iostream>
#include <string>
#include<stdio.h>
#include<unistd.h>
#include<time.h>
#include <fcntl.h>
#include <sys/mman.h>

using namespace cv;
using namespace std;

Capture cap[10];

cv::Mat Frame2Mat(const Frame& frame)
{
    float time=0;
//    std::cout<<frame.height<<"  "<<frame.width<<std::endl;
    cv::Mat nv12(frame.height * frame.channels / frame.split, frame.width, CV_8UC1);
    struct timeval last={0,0},current={0,0};
    gettimeofday(&last,NULL);
    memcpy(nv12.data, frame.data, frame.width * frame.height * frame.channels / frame.split);
    gettimeofday(&current,NULL);
    time = (current.tv_sec - last.tv_sec) * 1000000
        + current.tv_usec - last.tv_usec;
    printf("time:%f\r\n",time);
    cv::Mat bgr;
    cv::cvtColor(nv12, bgr, cv::COLOR_YUV2RGB_NV12);
    return bgr;
}
cv::Mat Frame2UYVYMat4to1(const Frame& frame)
{
    std::cout<<frame.height<<"  "<<frame.width<<"  "<<frame.channels<<"  "<<frame.split<<std::endl;
    cv::Mat uyvy(frame.height, frame.width, CV_8UC2);  //uyvy format define
    memcpy(uyvy.data, frame.data, frame.width * frame.height * frame.channels / frame.split);
//    for(int i=0;i<(frame.height/4);i++){
//        memcpy(uyvy.data+i*frame.width *2, frame.data+i*4*2*frame.width, frame.width *2);
//        memcpy(uyvy.data+frame.width*2*720+i*frame.width *2, frame.data+i*4*2*frame.width+1*frame.width *2, frame.width *2);
//        memcpy(uyvy.data+frame.width*2*1440+i*frame.width *2, frame.data+i*4*2*frame.width+2*frame.width *2, frame.width *2);
//        memcpy(uyvy.data+frame.width*2*2160+i*frame.width *2, frame.data+i*4*2*frame.width+3*frame.width *2, frame.width *2);
//    }
    cv::Mat bgr;
    cv::cvtColor(uyvy, bgr, cv::COLOR_YUV2BGR_UYVY);
    return bgr;
}

cv::Mat Frame2UYVYMat4to1_test(const Frame& frame)
{
    cv::Mat bgr;
    std::cout<<frame.height<<"  "<<frame.width<<"  "<<frame.channels<<"  "<<frame.split<<std::endl;
    cv::Mat uyvy(720*4, 1280, CV_8UC2);  //uyvy format define
//    memcpy(uyvy.data, frame.data, frame.width * frame.height * frame.channels / frame.split);

    unsigned int frame0_index=0,frame1_index=0,frame2_index=0,frame3_index=0;
//    printf("frame data1 : %x \r\n",frame.data[frame.width *2*1-1]);
    for(int i=0;i<(frame.height);i++){
//        memcpy(uyvy.data+i*frame.width *2, frame.data+i*4*2*frame.width, frame.width *2);
//        memcpy(uyvy.data+frame.width*2*720+i*frame.width *2, frame.data+i*4*2*frame.width+1*frame.width *2, frame.width *2);
//        memcpy(uyvy.data+frame.width*2*1440+i*frame.width *2, frame.data+i*4*2*frame.width+2*frame.width *2, frame.width *2);
//        memcpy(uyvy.data+frame.width*2*2160+i*frame.width *2, frame.data+i*4*2*frame.width+3*frame.width *2, frame.width *2);

        if((frame.data[frame.width *2*i]==0xff)&&(frame.data[frame.width *2*i+1]==0xfe)){
            printf("tuser %d %d vc en :%d line: %d\r\n",frame.data[frame.width *2*(i)-3],frame.data[frame.width *2*(i+1)-3],frame.data[frame.width *2*(i+1)-1],i);
        }

//        switch (frame.data[frame.width *2*(i+1)-1]){
//            case 0 :
//                memcpy(uyvy.data+frame0_index*frame.width *2, frame.data+i*2*frame.width, frame.width *2);
//                frame0_index++;
//                break;
//            case 1 :
//                memcpy(uyvy.data+frame.width*2*720+frame1_index*frame.width *2, frame.data+i*2*frame.width, frame.width *2);
//                frame1_index++;
//                break;
//            case 2 :
//                memcpy(uyvy.data+frame.width*2*1440+frame2_index*frame.width *2, frame.data+i*2*frame.width, frame.width *2);
//                frame2_index++;
//                break;
//            case 3 :
//                memcpy(uyvy.data+frame.width*2*2160+frame3_index*frame.width *2, frame.data+i*2*frame.width, frame.width *2);
//                frame3_index++;
//                break;
//            default:
//            printf("erro cap virtual channel index\r\n");
//            break;
//        }
    }
    cv::cvtColor(uyvy, bgr, cv::COLOR_YUV2BGR_UYVY);
//    printf("tuser last data1 : %d  vc: %d \r\n",frame.last_data[frame.width *2*2879-3],frame.last_data[frame.width *2*2879-1]);
//    printf("tuser data1 : %d  vc: %d \r\n",frame.data[frame.width *2*1-3],frame.data[frame.width *2*1-1]);
//    printf("frame data1 : %x \r\n",frame.data[frame.width *2*1-1]);
//    printf("frame data2 : %x \r\n",frame.data[frame.width *2*2-1]);
//    printf("frame data3 : %x \r\n",frame.data[frame.width *2*3-1]);
//    printf("frame data4 : %x \r\n",frame.data[frame.width *2*4-1]);
//    printf("frame frame0_index : %d \r\n",frame0_index);
//    printf("frame frame1_index : %d \r\n",frame1_index);
//    printf("frame frame2_index : %d \r\n",frame2_index);
//    printf("frame frame3_index : %d \r\n",frame3_index);

    return bgr;
}
cv::Mat Frame2UYVYMat4to1_old(const Frame& frame)
{
    cv::Mat bgr;
//    std::cout<<frame.height<<"  "<<frame.width<<"  "<<frame.channels<<"  "<<frame.split<<std::endl;
    cv::Mat uyvy(720*4, 1280, CV_8UC2);  //uyvy format define

    unsigned int frame0_index=0,frame1_index=0,frame2_index=0,frame3_index=0;
//    printf("frame data1 : %x \r\n",frame.data[frame.width *2*1-1]);
    struct timeval last={0,0},current={0,0};
    gettimeofday(&last,NULL);

    for(int i=0;i<(frame.height);i++){
//        if((frame.data[frame.width *2*i]==0xff)&&(frame.data[frame.width *2*i+1]==0xfe)){
//            printf("tuser %d %d vc en :%d line: %d\r\n",frame.data[frame.width *2*(i)-3],frame.data[frame.width *2*(i+1)-3],frame.data[frame.width *2*(i+1)-1],i);
//        }
        switch (frame.data[frame.width *2*(i+1)-1]){
            case 0 :
                memcpy(uyvy.data+frame0_index*frame.width *2, frame.data+i*2*frame.width, frame.width *2);
                frame0_index++;
                break;
            case 1 :
                memcpy(uyvy.data+frame.width*2*720+frame1_index*frame.width *2, frame.data+i*2*frame.width, frame.width *2);
                frame1_index++;
                break;
            case 2 :
                memcpy(uyvy.data+frame.width*2*1440+frame2_index*frame.width *2, frame.data+i*2*frame.width, frame.width *2);
                frame2_index++;
                break;
            case 3 :
                memcpy(uyvy.data+frame.width*2*2160+frame3_index*frame.width *2, frame.data+i*2*frame.width, frame.width *2);
                frame3_index++;
                break;
            default:
            printf("erro cap virtual channel index\r\n");
            break;
        }
    }

    cv::cvtColor(uyvy, bgr, cv::COLOR_YUV2BGR_UYVY);


//    printf("tuser last data1 : %d  vc: %d \r\n",frame.last_data[frame.width *2*2879-3],frame.last_data[frame.width *2*2879-1]);
//    printf("tuser data1 : %d  vc: %d \r\n",frame.data[frame.width *2*1-3],frame.data[frame.width *2*1-1]);
//    printf("frame data1 : %x \r\n",frame.data[frame.width *2*1-1]);
//    printf("frame data2 : %x \r\n",frame.data[frame.width *2*2-1]);
//    printf("frame data3 : %x \r\n",frame.data[frame.width *2*3-1]);
//    printf("frame data4 : %x \r\n",frame.data[frame.width *2*4-1]);
//    printf("frame frame0_index : %d \r\n",frame0_index);
//    printf("frame frame1_index : %d \r\n",frame1_index);
//    printf("frame frame2_index : %d \r\n",frame2_index);
//    printf("frame frame3_index : %d \r\n",frame3_index);

    return bgr;
}

cv::Mat Frame2UYVYMat4to1_all(const Frame& frame)
{
    cv::Mat bgr;
//    std::cout<<frame.height<<"  "<<frame.width<<"  "<<frame.channels<<"  "<<frame.split<<std::endl;
    cv::Mat uyvy(720*4, 1280, CV_8UC2);  //uyvy format define

    unsigned int frame_index[4]={0,0,0,0};
    unsigned int line_vc=0;
    bool vc_enable[4]={false,false,false,false};
    for(int i=1;i<(frame.height);i++){
        line_vc=frame.last_data[frame.width *2*(i+1)-1];
        if((frame.last_data[frame.width *2*i]==0xff)&&(frame.last_data[frame.width *2*i+1]==0xfe)){
            vc_enable[line_vc]=true;
//            printf("tuser count %d %d vc en :%d line: %d\r\n",frame.last_data[frame.width *2*(i)-3],frame.last_data[frame.width *2*(i+1)-3],line_vc,i);
        }

        if(vc_enable[line_vc]){
            memcpy(uyvy.data+frame_index[line_vc]*frame.width *2+line_vc*1280*2*720, frame.last_data+i*2*frame.width, frame.width *2);
            frame_index[line_vc]++;
        }
        else{
            continue;
        }
    }
    line_vc=frame.data[frame.width *2-1];
    vc_enable[line_vc]=true;
//    printf("                linevc en :%d\r\n",line_vc);
    for(int i=0;i<(frame.height);i++){
        line_vc=frame.data[frame.width *2*(i+1)-1];
//        if((frame.data[frame.width *2*i]==0xff)&&(frame.data[frame.width *2*i+1]==0xfe)){
//            vc_enable[line_vc]=true;
//            printf("tuser count %d %d vc en :%d line: %d\r\n",frame.data[frame.width *2*(i)-3],frame.data[frame.width *2*(i+1)-3],line_vc,i);
//        }
        if(frame_index[line_vc]<720){
            memcpy(uyvy.data+frame_index[line_vc]*frame.width *2+line_vc*1280*2*720, frame.data+i*2*frame.width, frame.width *2);
            frame_index[line_vc]++;
        }
        else{
            continue;
        }
    }
//    for(int i=0;i<4;i++){
//        printf("frame_index %d \r\n",frame_index[i]);
//    }
    cv::cvtColor(uyvy, bgr, cv::COLOR_YUV2BGR_UYVY);
    return bgr;
}

cv::Mat Frame2UYVYMat4to1_one(const Frame& frame,unsigned char video_index)
{
    cv::Mat bgr;
//    std::cout<<frame.height<<"  "<<frame.width<<"  "<<frame.channels<<"  "<<frame.split<<std::endl;
    cv::Mat uyvy(720, 1280, CV_8UC2);  //uyvy format define

    if(video_index>4){
        video_index=0;
    }

    unsigned int frame_index[4]={0,0,0,0};
    unsigned int line_vc=0;
    bool vc_enable[4]={false,false,false,false};
    for(int i=1;i<(frame.height);i++){
        line_vc=frame.last_data[frame.width *2*(i+1)-1];
        if((frame.last_data[frame.width *2*i]==0xff)&&(frame.last_data[frame.width *2*i+1]==0xfe)){
            vc_enable[line_vc]=true;
//            printf("tuser count %d %d vc en :%d line: %d\r\n",frame.last_data[frame.width *2*(i)-3],frame.last_data[frame.width *2*(i+1)-3],line_vc,i);
        }

        if(vc_enable[line_vc]&&(video_index==line_vc)){
            memcpy(uyvy.data+frame_index[line_vc]*frame.width *2, frame.last_data+i*2*frame.width, frame.width *2);
            frame_index[line_vc]++;
        }
        else{
            continue;
        }
    }
    line_vc=frame.data[frame.width *2-1];
    vc_enable[line_vc]=true;
//    printf("                linevc en :%d\r\n",line_vc);
    for(int i=0;i<(frame.height);i++){
        line_vc=frame.data[frame.width *2*(i+1)-1];
//        if((frame.data[frame.width *2*i]==0xff)&&(frame.data[frame.width *2*i+1]==0xfe)){
//            vc_enable[line_vc]=true;
//            printf("tuser count %d %d vc en :%d line: %d\r\n",frame.data[frame.width *2*(i)-3],frame.data[frame.width *2*(i+1)-3],line_vc,i);
//        }
        if(frame_index[line_vc]<720&&(video_index==line_vc)){
            memcpy(uyvy.data+frame_index[line_vc]*frame.width *2, frame.data+i*2*frame.width, frame.width *2);
            frame_index[line_vc]++;
        }
        else{
            continue;
        }
    }
//    for(int i=0;i<4;i++){
//        printf("frame_index %d \r\n",frame_index[i]);
//    }
    cv::cvtColor(uyvy, bgr, cv::COLOR_YUV2BGR_UYVY);
    return bgr;
}



cv::Mat Frame2UYVYMat4to1_700(const Frame& frame)
{
    std::cout<<frame.height<<"  "<<frame.width<<"  "<<frame.channels<<"  "<<frame.split<<std::endl;
    cv::Mat uyvy(frame.height, frame.width, CV_8UC2);  //uyvy format define
//    memcpy(uyvy.data, frame.data, frame.width * frame.height * frame.channels / frame.split);
    for(int i=0;i<(frame.height/4);i++){
        memcpy(uyvy.data+i*frame.width *2, frame.data+i*4*2*frame.width, frame.width *2);
        memcpy(uyvy.data+frame.width*2*700+i*frame.width *2, frame.data+i*4*2*frame.width+1*frame.width *2, frame.width *2);
        memcpy(uyvy.data+frame.width*2*1400+i*frame.width *2, frame.data+i*4*2*frame.width+2*frame.width *2, frame.width *2);
        memcpy(uyvy.data+frame.width*2*2100+i*frame.width *2, frame.data+i*4*2*frame.width+3*frame.width *2, frame.width *2);
    }
    cv::Mat bgr;
    cv::cvtColor(uyvy, bgr, cv::COLOR_YUV2RGB_UYVY);
    return bgr;
}
cv::Mat Frame2YUYVMat(const Frame& frame)
{
    std::cout<<frame.height<<"  "<<frame.width<<"  "<<frame.channels<<"  "<<frame.split<<std::endl;
    cv::Mat yuyv(frame.height, frame.width, CV_8UC2);  //uyvy format define
    memcpy(yuyv.data, frame.data, frame.width * frame.height * frame.channels / frame.split);
    cv::Mat bgr;
    cv::cvtColor(yuyv, bgr, cv::COLOR_YUV2BGR_YUYV);
    return bgr;
}
cv::Mat Frame2BGRMat(const Frame& frame)
{
//    std::cout<<frame.height<<"  "<<frame.width<<std::endl;
    cv::Mat bgr(frame.height, frame.width, CV_8UC3);
    memcpy(bgr.data, frame.data, frame.width * frame.height * frame.channels / frame.split);
    return bgr;
}

int img_write_test(){
    for(int i=0;i<100;++i)
    {
        const Frame &tmp=cap[0].getLastFrame();
        if(tmp.data!=NULL){
            cv::Mat frame = Frame2Mat(tmp);
            std::stringstream ss;
            ss<<i<<".jpg";
            std::cout<<"frmae count : "<<i<<std::endl;
            cv::imwrite(ss.str(),frame);
        }
        else{
            std::cout<<"frame data is null"<<std::endl;
        }
        sleep(1);
    }
}

int data_capture_nv12(int argc, char **argv,int video_cnt,int width,int height)
{
    Mat bgr(height, width, CV_8UC3);
    Mat nv12[10];
    string filename;
    char *p[10];
    int caputure_count=0;

        for(int i=0;i<8;i++){
            cap[i].initCapture(i);
    //or
    //        cap[i].initCapture(i,V4L2_PIX_FMT_RGB24);
        }

    getchar();

    if (argc != 2) {
        printf("plaease input capture count \n");
        return -1;
    }

    caputure_count = atoi(argv[1]);
    printf("capture count:%d\n",caputure_count);
//    return 0;

    sleep(2);
    for(int i=0;i<video_cnt;i++){
        Mat tmp(height*3/2, width, CV_8UC1);
        nv12[i]=tmp;
    }
    for(int count=0;count<caputure_count;count++)
    {
//        std::cout << "data queue size : " <<data_queue.size()<< std::endl;
        for(int i=0;i<video_cnt;++i)
        {
            const Frame &tmp=cap[i].getLastFrame();
            if(tmp.data==nullptr){
                fprintf(stderr,"v4l2 capture  erro frame.data is null ");
                sleep(1);
                continue;
            }
            p[i]=(char *)tmp.data;
//            printf("stitch video:%d,buffer_index:%d add:%ld \r\n",i,index,p);
//              sleep(1);
        }
        for(int i=0;i<video_cnt;i++){
            memcpy(nv12[i].data,p[i], width*height*3/2);
        }
        for(int i=0;i<video_cnt;i++){
            filename="/home/root/gwj/"+to_string(i)+"-"+to_string(count)+".png";
            cvtColor(nv12[i], bgr, CV_YUV2RGB_NV12);
            imwrite(filename,bgr);
        }
        sleep(1);
        cout<<"count+++"<<count<<endl;
//        flush(stdout);
    }
}
int data_capture_yuyvall4()
{
    Mat nv12[10];
    string filename;
    char *p[10];
    int caputure_count=0;
//    Capture cap0=Capture(5120,720,V4L2_PIX_FMT_UYVY);
//    Capture cap0=Capture(1280,2880,V4L2_PIX_FMT_UYVY);
    Capture cap0=Capture(1280,2880,V4L2_PIX_FMT_UYVY);

    cap0.initCapture(0);

    sleep(2);

//    while(1);

    struct timeval last={0,0},current={0,0};
      double fps = 0;
      int count=0;

    for(int i=0;i<10;++i)
//    while(1)
    {
      gettimeofday(&current,NULL);
      //current.tv_sec * 1000 + current.tv_usec / 1000;
      fps = (current.tv_sec - last.tv_sec) * 1000000
          + current.tv_usec - last.tv_usec;
      fps = fps ? 1000000.0 / fps : 0.0;

        const Frame &tmp=cap0.getLastFrame();
        if(tmp.data!=NULL){
//            cv::Mat frame = Frame2UYVYMat4to1_all(tmp);
            cv::Mat frame = Frame2UYVYMat4to1_one(tmp,0);
            if(frame.data==nullptr){
                fprintf(stderr,"v4l2 capture  erro frame.data is null ");
                sleep(1);
                continue;
            }
            sleep(1);
            std::stringstream ss;
            ss<<i<<".jpg";
            std::cout<<"frmae count : "<<i<<std::endl;
            cv::imwrite(ss.str(),frame);

        }
        else{
            sleep(1);
            std::cout<<"frame data is null"<<std::endl;
        }

        count++;
        if(count%5==0){
        printf("copy speed :fps%f\n",fps);
//        fflush(stdout);
        }
        last=current;
    }
    sleep(5);
}

int data_capture_usb()
{
    Mat nv12[10];
    string filename;
    char *p[10];
    int caputure_count=0;
    Capture cap0=Capture(1280,720);
    cap0.format=V4L2_PIX_FMT_YUYV;
    //or
//    Capture cap0=Capture(1280,720,V4L2_PIX_FMT_YUYV);
    cap0.capture_type=V4L2_BUF_TYPE_VIDEO_CAPTURE;

    cap0.initCapture(0);

    sleep(2);
//    while(1);

    for(int i=0;i<10;++i)
    {
        const Frame &tmp=cap0.getLastFrame();
        if(tmp.data!=NULL){
            cv::Mat frame = Frame2YUYVMat(tmp);
            if(frame.data==nullptr){
                fprintf(stderr,"v4l2 capture  erro frame.data is null ");
                sleep(1);
                continue;
            }
            std::stringstream ss;
            ss<<i<<".jpg";
            std::cout<<"frmae count : "<<i<<std::endl;
            cv::imwrite(ss.str(),frame);
        }
        else{
            std::cout<<"frame data is null"<<std::endl;
        }
        sleep(1);
    }
    sleep(5);
}

void cap_bgr_img(){
    int ret;
    setvbuf(stdout,NULL,_IOLBF,0);

//    for(int i=0;i<8;i++){
//        cap[i].initCapture(i);
////or
////        cap[i].initCapture(i,V4L2_PIX_FMT_RGB24);
//    }

//    img_write_test();
//    ret=data_capture_nv12(argc,argv,8,1280,720);

    Capture record=Capture(1920,1080,V4L2_PIX_FMT_BGR24);
    record.initCapture(9);

    for(int i=0;i<5;++i)
    {
        const Frame &tmp=record.getLastFrame();
        if(tmp.data!=NULL){
            cv::Mat frame = Frame2BGRMat(tmp);
            if(frame.data==nullptr){
                fprintf(stderr,"v4l2 capture  erro frame.data is null ");
                sleep(1);
                continue;
            }
            std::stringstream ss;
            ss<<i<<".jpg";
            std::cout<<"frmae count : "<<i<<std::endl;
            cv::imwrite(ss.str(),frame);
        }
        else{
            std::cout<<"frame data is null"<<std::endl;
        }
        sleep(1);
    }
    sleep(3);


}



void cap_test(){

    int fd,buf_size=256*1024*1024;
    unsigned char *buf;

    setvbuf(stdout,NULL,_IOLBF,0);

    Capture cap=Capture(1280,720);
    cap.initCapture(0);
    sleep(1);

    for(int i=0;i<100;++i)
    {
        const Frame &tmp=cap.getLastFrame();
        if(tmp.data!=NULL){
            cv::Mat frame = Frame2Mat(tmp);
            std::stringstream ss;
            ss<<i<<".jpg";
            std::cout<<"frmae count : "<<i<<std::endl;
            cv::imwrite(ss.str(),frame);
        }
        else{
            std::cout<<"frame data is null"<<std::endl;
        }
        sleep(1);
    }
    sleep(5);
}

void cap_enum_video_formats(){
        video_enum_formats("/dev/video0");
        video_enum_formats("/dev/video6");
        while(1){
            sleep(1);
        }
}

int main(int argc, char **argv)
{
    setvbuf(stdout,NULL,_IOLBF,0);


//    cap_test();

//    cap_enum_video_formats();
//    cap_bgr_img();
    data_capture_nv12(argc,argv,6,1280,720);
//    data_capture_yuyvall4();
//    data_capture_usb();
    while(1);
    return 0;
}
